#include <stdio.h>
#include <stdlib.h>
#include <GL/gl.h>
#include "vertex_processor.h"
#include "assembler.h"

MATRIX4 direct[4];
MATRIX4 inverse[4];
VECTOR4 color;

//Carrega matriz identidade

void sqsLoadIdentity(int index) {
    sqsIDENTITYMATRIX4(direct[index]);
    sqsIDENTITYMATRIX4(inverse[index]);
}

/*Faz o escalamento na matriz direct e inverse, multiplicando-as por uma matriz de escalamento(mat)
###########################################################
           (direct) mat = |sx   0   0   0|
                          |0   sy   0   0|
                          |0    0   sz  0|
                          |0    0   0   1|
###########################################################
          (inverse) mat = |1/sx   0   0     0|
                          |0    1/sy  0     0|
                          |0      0   1/sz  0|
                          |0      0   0     1|
###########################################################
 */
void sqsScale(int index, float sx, float sy, float sz) {
    if ((sx != 0) && (sy != 0) && (sz != 0)) {
        MATRIX4 mat;
        sqsSETMATRIX4(mat, sx, 0, 0, 0, 0, sy, 0, 0, 0, 0, sz, 0, 0, 0, 0, 1);
        sqsMultMatrix4(direct[index], direct[index], mat);
        sqsSETMATRIX4(mat, 1 / sx, 0, 0, 0, 0, 1 / sy, 0, 0, 0, 0, 1 / sz, 0, 0, 0, 0, 1);
        sqsMultMatrix4(inverse[index], mat, inverse[index]);
    }
}

/*Faz a transla�ao na matriz direct e inverse, multiplicando-as por uma matriz de transla�ao(mat)
###########################################################
          (direct)  mat = |1   0   0   tx|
                          |0   1   0   ty|
                          |0   0   1   tz|
                          |0   0   0    1|
    sqsPrintMatrix(direct[MODELMATRIX], NULL);
    exit(0);
###########################################################
          (inverse) mat = |1   0   0   -tx|
                          |0   1   0   -ty|
                          |0   0   1   -tz|
                          |0   0   0     1|
###########################################################
 */
void sqsTranslate(int index, float tx, float ty, float tz) {
    MATRIX4 mat;
    sqsSETMATRIX4(mat, 1, 0, 0, tx, 0, 1, 0, ty, 0, 0, 1, tz, 0, 0, 0, 1);
    sqsMultMatrix4(direct[index], direct[index], mat);
    sqsSETMATRIX4(mat, 1, 0, 0, -tx, 0, 1, 0, -ty, 0, 0, 1, -tz, 0, 0, 0, 1);
    sqsMultMatrix4(inverse[index], mat, inverse[index]);
}

/*Faz a rotacao nos exos X, Y, Z na matriz direct e inverse, multiplicando-as por uma matriz de Rota�ao(mat)
construida utilizando o angulo de rota�ao desejado, passado por parametro
################################################################################################################################
   (direct) EIXO X  mat = |1    0      0    0|      EIXO Y  mat = |cos   0    sen    0|      EIXO Z  mat = |cos   -sen   0    0|
                          |0   cos   -sen   0|                    |0     1     0     0|                    |sen    cos   0    0|
                          |0   sen    cos   0|                    |-sen  0    cos    0|                    |0       0    1    0|
                          |0    0      0    1|                    |0     0     0     1|                    |0       0    0    1|
################################################################################################################################
Na inverse � s� calcular o seno e cosseno do angulo negativo, as estruturas das matrizes sao as mesmas
################################################################################################################################
  (inverse) EIXO X  mat = |1    0      0    0|      EIXO Y  mat = |cos   0    sen    0|      EIXO Z  mat = |cos   -sen   0    0|
                          |0   cos   -sen   0|                    |0     1     0     0|                    |sen    cos   0    0|
                          |0   sen    cos   0|                    |-sen  0    cos    0|                    |0       0    1    0|
                          |0    0      0    1|                    |0     0     0     1|                    |0       0    0    1|
################################################################################################################################
 */

void sqsRotateX(int index, float angle) {
    MATRIX4 mat;
    angle = sqsRadianos(angle);
    float seno = sin(angle);
    float cosseno = cos(angle);
    sqsSETMATRIX4(mat, 1, 0, 0, 0, 0, (float) cosseno, (float) - seno, 0, 0, (float) seno, (float) cosseno, 0, 0, 0, 0, 1);
    sqsMultMatrix4(direct[index], direct[index], mat);
    seno = sin(-angle);
    cosseno = cos(-angle);
    sqsSETMATRIX4(mat, 1, 0, 0, 0, 0, (float) cosseno, (float) - seno, 0, 0, (float) seno, (float) cosseno, 0, 0, 0, 0, 1);
    sqsMultMatrix4(inverse[index], mat, inverse[index]);
}

void sqsRotateY(int index, float angle) {
    MATRIX4 mat;
    angle = sqsRadianos(angle);
    float seno = sin(angle);
    float cosseno = cos(angle);
    sqsSETMATRIX4(mat, (float) cosseno, 0, (float) seno, 0, 0, 1, 0, 0, (float) - seno, 0, (float) cosseno, 0, 0, 0, 0, 1);
    sqsMultMatrix4(direct[index], direct[index], mat);
    seno = sin(-angle);
    cosseno = cos(-angle);
    sqsSETMATRIX4(mat, (float) cosseno, 0, (float) seno, 0, 0, 1, 0, 0, (float) - seno, 0, (float) cosseno, 0, 0, 0, 0, 1);
    sqsMultMatrix4(inverse[index], mat, inverse[index]);
}

void sqsRotateZ(int index, float angle) {
    MATRIX4 mat;
    angle = sqsRadianos(angle);
    float seno = sin(angle);
    float cosseno = cos(angle);
    sqsSETMATRIX4(mat, (float) cosseno, (float) - seno, 0, 0, (float) seno, (float) cosseno, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
    sqsMultMatrix4(direct[index], direct[index], mat);
    seno = sin(-angle);
    cosseno = cos(-angle);
    sqsSETMATRIX4(mat, (float) cosseno, (float) - seno, 0, 0, (float) seno, (float) cosseno, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
    sqsMultMatrix4(inverse[index], mat, inverse[index]);
}

/*Seta a matriz de proje�ao ortogonal na matriz direct e inverse
##############################################################################################################################
          direct[PROJECTIONMATRIX] = |2/(rigth - left)           0                0            -(rigth + left)/(right - left)|
                                     |0                  2/(top - bottom)         0            -(top + bottom)/(top - bottom)|
                                     |0                          0         -2/far - near)      -(far + near)/(far - near)    |
                                     |0                          0                0                           1              |
##############################################################################################################################
          inverse[PROJECTIONMATRIX] =|(rigth - left)/2           0                0            (rigth + left)/2 |
                                     |0                  (top - bottom)/2         0            (top + bottom)/2 |
                                     |0                          0          (far - near)/-2    (near + far)/2   |
                                     |0                          0                0                           1 |
##############################################################################################################################
 */

void sqsOrthogonal(float l, float r, float t, float b, float n, float f) {
    if (((r - l) != 0.0f) && ((t - b) != 0.0f) && ((f - n) != 0.0f)) {
        sqsSETMATRIX4(direct[PROJECTIONMATRIX], (float) (2 / (r - l)), 0.0f, 0.0f, (float) (-(r + l) / (r - l)), 0.0f, (float) (2 / (t - b)), 0.0f, (float) (-(t + b) / (t - b)), 0.0f, 0.0f, (float) (-2 / (f - n)), (float) (-(f + n) / (f - n)), 0.0f, 0.0f, 0.0f, 1.0f);
        sqsSETMATRIX4(inverse[PROJECTIONMATRIX], (float) ((r - l) / 2), 0.0f, 0.0f, (float) ((r + l) / 2), 0.0f, (float) ((t - b) / 2), 0.0f, (float) ((t + b) / 2), 0.0f, 0.0f, (float) ((f - n) / -2), (float) ((n + f) / 2), 0.0f, 0.0f, 0.0f, 1.0f);
    }
}

/*Realiza a transformacao que ajusta o frustum na matriz direct e inverse
##################################################################################################################################################################
          direct[PROJECTIONMATRIX] = |(2 * near)/(rigth - left)             0                    (rigth + left)/(right - left)                   0               |
                                     |0                          (2 * near)/(top - bottom)       (top + bottom)/(top - bottom)                   0               |
                                     |0                                     0                   -(far + near)/(far - near)       -(2 * far * near) / (far - near)|
                                     |0                                     0                                -1                                  0               |
##################################################################################################################################################################
          inverse[PROJECTIONMATRIX] =|(rigth - left)/(2 * near)                 0                             0                        (rigth + left)/(2 * near) |
                                     |0                           (top - bottom)/(2 * near)                   0                        (top + bottom)/(2 * near) |
                                     |0                                         0                             0                                     -1           |
                                     |0                                         0                -(far - near)/ 2 * far * near)     (far + near)/(2 * far * near)|
##################################################################################################################################################################
 */
void sqsFrustum(float l, float r, float t, float b, float n, float f) {
    if (((r - l) != 0.0f) && ((t - b) != 0.0f) && ((f - n) != 0) && (n != 0) && (f != 0)) {
        sqsSETMATRIX4(direct[PROJECTIONMATRIX], (float) ((2 * n) / (r - l)), 0.0f, (float) ((r + l) / (r - l)), 0.0f, 0.0f, (float) ((2 * n) / (t - b)), (float) ((t + b) / (t - b)), 0.0f, 0.0f, 0.0f, (float) (-(f + n) / (f - n)), (float) (-(2 * f * n) / (f - n)), 0.0f, 0.0f, -1.0f, 0.0f);
        sqsSETMATRIX4(inverse[PROJECTIONMATRIX], (float) ((r - l) / (2 * n)), 0.0f, 0.0f, (float) ((r + l) / (2 * n)), 0.0f, (float) ((t - b) / (2 * n)), 0.0f, (float) ((t + b) / (2 * n)), 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, (float) (-(f - n) / (2 * f * n)), (float) ((f + n) / (2 * f * n)));
    }
}

/*Seta a Transformacao Perspectiva na matriz direct e inverse
##################################################################################################################################################################
          direct[PROJECTIONMATRIX] = |cotangente/aspectratio          0                    0                                 0               |
                                     |0                           cotangente               0                                 0               |
                                     |0                               0        (far + near)/(near - far)     (2 * far * near)/(near - far)   |
                                     |0                               0                   -1                                 0               |
##################################################################################################################################################################
 */
void sqsPerspective(float fovh, float aspectratio, float near, float far) {
    fovh = sqsRadianos(fovh);
    if ((tan(fovh / 2) != 0) && (aspectratio != 0) && ((near - far) != 0)) {
        MATRIX4 mat, inv;
        float cotangente = (1 / tan(fovh / 2));
        sqsSETMATRIX4(mat, (float) (cotangente / aspectratio), 0.0f, 0.0f, 0.0f, 0.0f, cotangente, 0.0f, 0.0f, 0.0f, 0.0f, (float) ((far + near) / (near - far)), (float) ((2 * far * near) / (near - far)), 0.0f, 0.0f, -1.0f, 0.0f);
        sqsMultMatrix4(direct[PROJECTIONMATRIX], direct[PROJECTIONMATRIX], mat);
        sqsInverseMatrix4(inv, mat);
        sqsMultMatrix4(inverse[PROJECTIONMATRIX], inv, inverse[PROJECTIONMATRIX]);
        sqsScale(PROJECTIONMATRIX, 0.1, 0.1, 0.1);
    }
}

/*
Gera a matriz de visualiza�ao
 */
void sqsViewer(float ox, float oy, float oz, VECTOR3 center, VECTOR3 up) {
    MATRIX4 mat, inv;
    VECTOR3 eye, f, s, u;

    sqsLoadIdentity(VIEWMATRIX);

    sqsSETVECTOR3(eye, ox, oy, oz);
    sqsSUBVECTOR3(f, center, eye);
    sqsNormalizeVector3(f, f);

    sqsCOPYVECTOR3(u, up);
    sqsNormalizeVector3(u, u);

    sqsCROSSPRDUCT3(s, f, u);

    sqsCROSSPRDUCT3(u, f, s);

    sqsSETMATRIX4(mat, s[X], s[Y], s[Z], 0, u[X], u[Y], u[Z], 0, -f[X], -f[Y], -f[Z], 0, 0, 0, 0, 1);
    sqsMultMatrix4(direct[VIEWMATRIX], direct[VIEWMATRIX], mat);
    sqsTranslate(VIEWMATRIX, -ox, -oy, -oz);
    sqsInverseMatrix4(inv, mat);
    sqsMultMatrix4(inverse[VIEWMATRIX], inv, inverse[VIEWMATRIX]);
}

void sqsTransformGeometry(struct INTERNAL_VERTEX input, struct INTERNAL_VERTEX* output) {
    sqsMultMat4Vec4(output->normcoord, direct[MODELMATRIX], input.normcoord);
    sqsMultMat4Vec4(output->normcoord, direct[VIEWMATRIX], output->normcoord);
    sqsMultMat4Vec4(output->normcoord, direct[PROJECTIONMATRIX], output->normcoord);
    /*sqsMultMat4Vec3(output->normal, inverse[MODELMATRIX], input.normal);
    sqsMultMat4Vec3(output->normal, inverse[VIEWMATRIX], output->normal);
    sqsMultMat4Vec3(output->normal, inverse[PROJECTIONMATRIX], output->normal);*/
}

void sqsPushVertex(struct VERTEX v) {

    struct INTERNAL_VERTEX vert, outputvert;

    sqsCOPYVECTOR4(vert.normcoord, v.coord);
    sqsNormalizeVector3(vert.normal, v.normal);
    sqsCOPYVECTOR4(vert.color1, v.color);
    sqsSETVECTOR4(vert.color2, 0, 0, 0, 0);
    sqsCOPYVECTOR4(vert.texcoord1, v.texcoord1);

    sqsCloneVertex(&outputvert, &vert);

//    printf("antes: %f %f %f\n", outputvert.normcoord[X]/outputvert.normcoord[W], outputvert.normcoord[Y]/outputvert.normcoord[W], outputvert.normcoord[Z]/outputvert.normcoord[W]);

    sqsTransformGeometry(vert, &outputvert);
//    printf("depois: %f %f %f\n", outputvert.normcoord[X]/outputvert.normcoord[W], outputvert.normcoord[Y]/outputvert.normcoord[W], outputvert.normcoord[Z]/outputvert.normcoord[W]);
    sqsPrimitiveAssembler(outputvert);
}

void sqsPushVertex3f(float x, float y, float z) {
    struct VERTEX v;
    sqsSETVECTOR4(v.coord, x, y, z, 1);
    sqsCOPYVECTOR4(v.color, color);

    sqsPushVertex(v);
}

void sqsCloneVertex(struct INTERNAL_VERTEX *output, struct INTERNAL_VERTEX *input) {
    output->clipcode = input->clipcode;
    sqsCOPYVECTOR4(output->color1, input->color1);
    sqsCOPYVECTOR4(output->color2, input->color2);
    sqsCOPYVECTOR3(output->normal, input->normal);
    sqsCOPYVECTOR4(output->normcoord, input->normcoord);
    sqsCOPYVECTOR4(output->texcoord1, input->texcoord1);
}

void sqsColor4f(float r, float g, float b, float a) {
    color[0] = r;
    color[1] = g;
    color[2] = b;
    color[3] = a;
}

void sqsColor3f(float r, float g, float b) {
    sqsColor4f(r, g, b, 1.0);
}
